/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  PathPlanner
 */
 
 
 

#ifndef PATH_PLANNER_H_
#define PATH_PLANNER_H_

#include "ros/ros.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include "std_msgs/Bool.h"

#include "group6/goToNextGoal.h"
#include "group6/DetectedObject.h"
#include "group6/DetectedObjectArray.h"

#include <math.h>
#include <signal.h>

struct ToyGoal
{
    ToyGoal(double xCoord, double yCoord,int areaId, std::string toyId)
    {
        x = xCoord;
        y = yCoord;
        id = toyId;
        area = areaId;
    }

    double x;
    double y;
    int area; // 1 For close to basket 1, 2 for close to basket 2
    std::string id;
};

struct RandomPathGoal
{
    RandomPathGoal(double xCoord, double yCoord, double angle)
    {
        x = xCoord;
        y = yCoord;
        z = angle;
    }

    double x;
    double y;
    double z;

};

struct RandomPathGoalsInArea
{
    RandomPathGoalsInArea(){}

    RandomPathGoalsInArea(std::vector<RandomPathGoal> path, int areaId)
    {
        goalsInArea = path;
        area = areaId;
        currentIdx = 0;
        pathSize = goalsInArea.size();
        travelled = false;
    }

    std::vector<RandomPathGoal> goalsInArea;
    int area; // 1 For close to basket 1, 2 for close to basket 2
    int currentIdx;
    int pathSize;
    bool travelled;
};


typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

class PathPlanner
{
public:
    enum currentDestination {BASKET_1 = 1, BASKET_2 = 2,TOY = 3, RANDOM_SEARCH = 4};
    enum goalStatus {SUCCEED = 0, NOT_SUCCEED = 1 , NO_TOYS_LEFT = 2};

    enum specialCase{NONE = 0, BEHIND_BOX_1 = 1, BEHIND_BOX_2 = 2, AREA2_CLOSE_TO_WALL = 3, AREA2_BEHIND_BAR_RIGHT = 4, AREA2_BEHIND_BAR_LEFT = 5, AREA1_TO_AREA2 = 6, AREA2_TO_AREA1 = 7, AREA1_TO_AREA2_DOOR = 8};

    PathPlanner(ros::NodeHandle& nh);
    ~PathPlanner(){}

    bool nextGoalService(group6::goToNextGoal::Request &req, group6::goToNextGoal::Response &res);

    void updateObjectList(const group6::DetectedObjectArray::ConstPtr& msg);

    bool setNextGoal(int dest);

    //void setGoals(); // THIS IS JUST FOR MID TERM 1 & 2, BECAUSE THE GOAL POINTS ARE PREDEFINED

    bool proceedToNextGoal();

    static void signalHandler(int signal);

    //getters
    const int& getCurrentGoalIdx() const {return mCurrentGoalIdx;}
    const int& getTotalGoals() const {return mTotalGoals;}


private:  

    bool goToNextGoal(const double &xDirection, const double &yDirection, const double &angle);

    double calculateAngleForNextGoal(const double& currentX, const double& currentY);

    bool checkIfStuckedInPlace(const double& currentX, const double& currentY, const double &currentZ);

    bool checkIfCloseEnough(const double& currentX, const double& currentY);

    void addHelperGoal(const double& currentX, const double& currentY);

    bool getCurrentTransform(tf::StampedTransform &currentTransform);

    bool setNearestToyGoal();

    double getDistanceToToy(const tf::StampedTransform &currentTransform, const int& toyIdx);

    void adjustToyGoal(const tf::StampedTransform &currentTransform);

    int chooseArea(double x);

    bool randomSearch();

    void setSearchingPaths();

    void setRandomPathGoal();

    int checkWhichAreaToSearch();

    void updatePath();

    int checkHelperGoal(int area);

    void removeCurrentToy();

    enum specialCase checkForSpecialCases(const tf::StampedTransform &currentTransform);

    bool goToHelper(enum specialCase helper);

    void setToyBasket();

    //for debug
    void printToys();


    ros::NodeHandle mNodeHandle;
    MoveBaseClient mMoveBaseClient;
    move_base_msgs::MoveBaseGoal mNextGoal;

    tf::TransformListener mListener;
    ros::ServiceServer  mGoToGoalService;

    ros::Publisher mTriggerDetectPub;
    ros::Publisher mDeleteToyPub;

    std::vector<std::pair<double,double> > mGoals;
    std::pair<double, double> mCurrentGoal;
    int mCurrentToyBasket;
    geometry_msgs::Vector3 mToyToRemove;

    //std::vector<std::pair<double,double> > mToys;
    //mToys is a vector of pairs(toy_coordinates & toy_id)
    //std::vector<std::pair< std::pair <double,double>, std::string > > mToys;
    std::vector<ToyGoal> mToys;

    RandomPathGoalsInArea mArea1Path;
    RandomPathGoalsInArea mArea2Path;
    double mNextAngle;
    bool mRandomSearchInterrupted;

    int mCurrentGoalIdx;
    int mClosestToyIdx;
    int mTotalGoals;
    double mGoalTolerance;
    enum currentDestination mCurrentDestination;

    int mTimesInSamePosition; //helper for checking if the robot is "stucked" in place
    std::vector<double> mLastPositionAfterMovement; //helper for checking if the robot is "stucked" in place
    int mTimesTriedToSendSameGoal;

    int mToyCounter;
};

#endif
